/*!
 * Copyright (c) 2012
 * Automation and Robotics Lab (LARA) at University of Brasilia (UnB)
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     - Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     - Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     - Neither the name of the Automation and Robotics Lab (LARA) nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License LGPL as
 * published by the Free Software Foundation, either version 3 of the
 * License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU Lesser General Public License LGPL for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License LGPL along with this program.
 * If not, see <http://www.gnu.org/licenses/>.
 *
 *****************************************************************
 *
 *   ROS stack name: schunk
 *   ROS package name: schunk_low
 *
 *   Authors: André A. Geraldes, email: andregeraldes@lara.unb.br
 *            Murilo M. Marinho, email: murilomarinho@lara.unb.br
 *
 *****************************************************************/

#ifndef ROSPKG_SCHUNK_LOW_SCHUNK_H_
#define ROSPKG_SCHUNK_LOW_SCHUNK_H_

/******************************************************************************
                                   INCLUDES
 *****************************************************************************/

// General
#include <vector>
#include <string>
#include <pthread.h>

// ROS
#include <ros/ros.h>

// Messages
#include <schunk_msgs/AllParameters.h>
#include <schunk_msgs/JointStateComplete.h>
#include <schunk_msgs/JointStateElectrical.h>

/******************************************************************************
                                 CLASS SCHUNK
 *****************************************************************************/

class Schunk
{
    private:

        // Mutex
        pthread_mutex_t mutex_;

        // Robot Device Identifier
        int device_;

        // CAN Device Parameters
        std::string can_module_;
        std::string can_device_;
        int can_baudrate_;

        // Robot Parameters
        int module_count_;
        std::vector<int> module_ids_;
        std::vector<std::string> module_names_;
        std::vector<double> max_positions_;
        std::vector<double> min_positions_;
        std::vector<double> home_offsets_;
        std::vector<double> home_positions_;
        std::vector<double> home_velocities_;
        std::vector<double> max_velocities_;
        std::vector<double> max_accelerations_;

        // PID Parameters
        std::vector<int> A0_;
        std::vector<int> C0_;
        std::vector<int> Damp_;

        // Status Variables
        bool can_parameters_set_;
        bool robot_parameters_set_;
        bool pid_parameters_set_;

        bool initialized_;
        bool can_device_opened_;

        // Status Messages
        schunk_msgs::AllParameters param_msg_;
        schunk_msgs::JointStateComplete joint_msg_;
        schunk_msgs::JointStateElectrical elec_msg_;

        // Methods for sending the parameters to the hardware
        void SendPIDParameters();
        bool send_max_positions();
        bool send_min_positions();
        bool send_home_offsets();
        bool send_home_velocities();
        bool send_max_velocities();
        bool send_max_accelerations();

        // Internal Methods
        bool OpenDevice();
        bool ProbeModules();
        void InitMessages();
        void PrintParameters();

    public:

        // Constructor
        explicit Schunk(ros::NodeHandle n);

        // Destructor
        ~Schunk();

        // Methods for accessing member variables
        std::string can_module() { return this->can_module_; }
        std::string can_device() { return this->can_device_; }
        int can_baudrate() { return this->can_baudrate_; }

        int module_count() { return this->module_count_;}
        std::vector<int> module_ids() { return this->module_ids_; }
        std::vector<std::string> module_names() { return this->module_names_; }

        std::vector<double> max_positions() { return this->max_positions_; }
        std::vector<double> min_positions() { return this->min_positions_; }
        std::vector<double> home_offsets() { return this->home_offsets_; }
        std::vector<double> home_positions() { return this->home_positions_; }
        std::vector<double> home_velocities() { return this->home_velocities_; }
        std::vector<double> max_velocities() { return this->max_velocities_; }
        std::vector<double> max_accelerations() { return this->max_accelerations_; }

        std::vector<int> A0() { return this->A0_; }
        std::vector<int> C0() { return this->C0_; }
        std::vector<int> Damp() { return this->Damp_; }

        schunk_msgs::AllParameters param_msg() { return this->param_msg_; }
        schunk_msgs::JointStateComplete joint_msg() { return this->joint_msg_; }
        schunk_msgs::JointStateElectrical elec_msg() { return this->elec_msg_; }

        // Methods for setting member variables
        bool SetCanParameters(std::string can_module, std::string can_device, int can_baudrate);
        bool SetRobotParameters(const int& module_count, const std::vector<int>& module_ids, const std::vector<std::string>& module_names,
                                const std::vector<double>& max_positions, const std::vector<double>& min_positions,
                                const std::vector<double>& home_offsets, const std::vector<double>& home_positions, const std::vector<double>& home_velocities,
                                const std::vector<double>& max_velocities, const std::vector<double>& max_accelerations);
        bool SetPIDParameters(const std::vector<int>& A0, const std::vector<int>& C0, const std::vector<int>& Damp);

        bool set_max_positions(const std::vector<double>& max_positions);
        bool set_min_positions(const std::vector<double>& min_positions);
        bool set_home_offsets(const std::vector<double>& home_offsets);
        bool set_home_positions(const std::vector<double>& home_positions);
        bool set_home_velocities(const std::vector<double>& home_velocities);
        bool set_max_velocities(const std::vector<double>& max_velocities);
        bool set_max_accelerations(const std::vector<double>& max_accelerations);

        // Methods for reading robot's state
        void GetCurrentPositions(std::vector<double> *positions);
        std::vector<double> GetCurrentPositions();
        void GetCurrentVelocities(std::vector<double> *velocities);
        std::vector<double> GetCurrentVelocities();
        std::vector<int> GetCurrentA0();
        std::vector<int> GetCurrentC0();
        std::vector<int> GetCurrentDamp();
        std::vector<int> GetCurrentStates();

        // Methods for initializing and configuring the robot's state
        bool Init();
        bool InitGripper();
        bool Close();
        bool ResetAllModules();
        bool ResetModule(int module_id);
        bool SetSyncMotion();
        bool SetASyncMotion();

        // Methods for moving the robot
        bool Move();
        bool Stop();
        bool Reset();
        bool Reset(int module_id);
        bool DoHoming();
        bool DoHoming(int module_id);
        bool DoHardHoming(int module_count);
        bool FinishedMoving();

        void SetNextPosture(std::vector<double> pos, std::vector<double> vel, std::vector<double> acc);
        void SetVelocitiesForNextPosture(std::vector<double> vel);
        void SetJointValueForNextMove(int module_id, double pos, double vel, double acc);
        void SetJointVelocityForNextMove(int module_id, double vel);

        // Methods for storing robot's information in messages
        schunk_msgs::AllParameters ReadAllParameters();
        schunk_msgs::JointStateComplete ReadJointStates();
        schunk_msgs::JointStateElectrical ReadElectricalStates();
};

#endif  // ROSPKG_SCHUNK_LOW_SCHUNK_H_
